/*
 * console.c
 *
 *  Created on: 2020年11月22日
 *      Author: BZL
 */
#include <string.h>
#include "typedef.h"
#include "console.h"
#include "cmsis_os2.h"
#include "FreeRTOS.h"
#include "motor_task.h"

#define CONSOLE_BUFFER_SIZE		100

typedef struct _ConsoleCmd
{
	char *buffer;
	uint16_t readPos;
	uint16_t len;
}ConsoleCmd;

static ConsoleCmd gs_tConsoleCmd;

osThreadId_t g_tConsoleTaskHandle;
const osThreadAttr_t g_tConsoleTaskAttr = {
  .name = "ConsoleTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};

static int printf_all_command(char *cmd);
static int console_change_motor_mode(char *cmd);
static int console_torque_set(char *cmd);
static int console_speed_set(char *cmd);
static int console_position_set(char *cmd);
static int console_runctrl_set(char *cmd);
static int console_motor_stop(char *cmd);

#ifdef VIRTUAL_VOLTAGE_DEBUG
static int battery_voltage_debug(char *cmd);
#endif

const ConsoleForm gs_aConsoleForm[] =
{
	{"HELP", 0, printf_all_command, "printf all command"},
	{"AT+Mode", 0, console_change_motor_mode, "change motor mode"},
	{"AT+Torque", 0, console_torque_set, "set torque"},
	{"AT+Speed", 0, console_speed_set, "set speed"},
	{"AT+Pos", 0, console_position_set, "set position"},
	{"AT+RunCtrl", 0, console_runctrl_set, "set run ctrl mode params"},
	{"AT+Stop", 0, console_motor_stop, "motor stop"},
};

/******************************************************************************************
 * @brief   获取下一个参数的起始地址
 * @param	无
 * @retval	指向下一个参数的指针
 ******************************************************************************************/
char *get_parameter(void)
{
	while(gs_tConsoleCmd.buffer[gs_tConsoleCmd.readPos] != '\0')
	{
		if(gs_tConsoleCmd.buffer[gs_tConsoleCmd.readPos] == ' ')
		{
			gs_tConsoleCmd.readPos++;
			return &gs_tConsoleCmd.buffer[gs_tConsoleCmd.readPos];
		}
		else
		{
			gs_tConsoleCmd.readPos++;
		}
	}

	return (char *)0;
}

int32_t char2numberhex(char *param)
{
    uint32_t result = 0;
    uint16_t pos = 0;
    if(param[0] == '-')
    {
    	pos = 1;
    }
    for(uint8_t i=pos; i < sizeof(uint32_t); i++)
    {
        if(param[i] >= '0' && param[i] <= '9')
        {
            result <<= 4;
            result += param[i] - '0';
        }
        else if(param[i] >= 'a' && param[i] <= 'z')
        {
            result <<= 4;
            result += 10 + param[i] - 'a';
        }
        else if(param[i] >= 'A' && param[i] <= 'Z')
        {
            result <<= 4;
            result += 10 + param[i] - 'A';
        }
        else
        {
            break;
        }
    }

    if(pos == 1)
    {
    	result = -result;
    }

    return result;    
}

int32_t char2number(char *param)
{
    int32_t result = 0;
    uint8_t pos = 0;
	if(param[0] == '-')
	{
		pos = 1;
	}
    for(uint8_t i=pos; i < sizeof(uint32_t)+pos; i++)
    {
        if(param[i] >= '0' && param[i] <= '9')
        {
            result *= 10;
            result += param[i] - '0';
        }
        else
        {
        	break;
        }
    }

	if(pos)
	{
		result = result * -1;
	}

    return result;
}

/******************************************************************************************
 * @brief   查看所有指令
 * @param	cmd：指令数据
 * @retval	0-执行成功，-1-执行失败
 ******************************************************************************************/
static int printf_all_command(char *cmd)
{
	LOGI("", "Command   ");
	LOGI("", "Describe\n");
	for(uint8_t i=0; i < sizeof(gs_aConsoleForm)/sizeof(ConsoleForm); i++)
	{
		LOGI("", "%s  ", gs_aConsoleForm[i].name);
		LOGI("", "%s  \n", gs_aConsoleForm[i].describe);
	}

	return 0;
}

static int console_change_motor_mode(char *cmd)
{
	char *param = get_parameter();
	uint32_t id = char2number(param);
	param = get_parameter();
	uint32_t mode = char2number(param);
	LOGD("", "id:%ld  %ld", id, mode);
	motor_control_mode(id, mode);

	return 0;
}

static int console_torque_set(char *cmd)
{
	char *param = get_parameter();
	uint32_t id = char2number(param);
	param = get_parameter();
	uint32_t temp = char2number(param);
	float torque = temp / 100.0f;
	LOGD("", "id:%ld  %ld", id, temp);
	motor_task_torque_set(id, torque);

	return 0;
}

static int console_speed_set(char *cmd)
{
	char *param = get_parameter();
	int32_t id = char2number(param);
	param = get_parameter();
	int32_t temp = char2number(param);
	float speed = temp;						//RPM
	LOGD("", "id:%ld  %ld", id, temp);
	motor_task_speed_set(id, speed);

	return 0;
}

static int console_position_set(char *cmd)
{
	char *param = get_parameter();
	int32_t id = char2number(param);
	param = get_parameter();
	int32_t temp = char2number(param);
	float pos = temp;						//RPM
	LOGD("", "id:%ld  %ld", id, temp);
	motor_task_position_set(id, pos);

	return 0;
}

static int console_runctrl_set(char *cmd)
{
	char *param = get_parameter();
	int32_t id = char2number(param);

	param = get_parameter();
	int32_t pos = char2number(param);

	param = get_parameter();
	int32_t speed = char2number(param);

	param = get_parameter();
	int32_t kp = char2number(param);

	param = get_parameter();
	int32_t ki = char2number(param);

	LOGD("", "id:%ld\t%ld\t%ld\t%ld\t%ld", id, pos, speed, kp, ki);
	motor_task_runctrl_set(id, pos*1.0f, speed*1.0f, kp*1.0f, ki/100.0f);

	return 0;
}

static int console_motor_stop(char *cmd)
{
	char *param = get_parameter();
	uint32_t id = char2number(param);
	motor_stop(id);

	return 0;
}

void console_buffer_input(char *pData, uint16_t len)
{
	gs_tConsoleCmd.len = len > CONSOLE_BUFFER_SIZE ? CONSOLE_BUFFER_SIZE : len;
	memcpy(gs_tConsoleCmd.buffer, pData, gs_tConsoleCmd.len);
}


/******************************************************************************************
 * @brief   控制台扫描接收的指令
 * @param	无
 * @retval	无
 ******************************************************************************************/
void console_task(void *argument)
{
	uint16_t sLen; 

	while(1)
	{
		osDelay(20);
		if(SEGGER_RTT_HASDATA(0) > 3)
		{
			sLen = SEGGER_RTT_Read(0, gs_tConsoleCmd.buffer, CONSOLE_BUFFER_SIZE);
		}
		else
		{
			continue;
		}

		BOOL search = FALSE;
		gs_tConsoleCmd.buffer[sLen] = '\0';
		for(uint8_t i=0; i < sizeof(gs_aConsoleForm)/sizeof(ConsoleForm); i++)
		{
			if(memcmp(gs_aConsoleForm[i].name, gs_tConsoleCmd.buffer, strlen(gs_aConsoleForm[i].name)) == 0)
			{
				gs_tConsoleCmd.readPos = 0;
				gs_aConsoleForm[i].func(gs_tConsoleCmd.buffer);
				search = TRUE;
				break;
			}
		}

		if(!search)
		{
			LOGI("","the cmd was not found cmd:%s", gs_tConsoleCmd.buffer);
		}
	}
}

/******************************************************************************************
 * @brief   控制台初始化
 * @param	无
 * @retval	无
 ******************************************************************************************/
void console_init(void)
{
	gs_tConsoleCmd.buffer = (char *)pvPortMalloc(CONSOLE_BUFFER_SIZE);
	gs_tConsoleCmd.readPos = 0;
	g_tConsoleTaskHandle = osThreadNew(console_task, NULL, &g_tConsoleTaskAttr);
}

